#include "simple_nav_client.hpp"

using namespace std::chrono_literals;

SimpleNavClient::SimpleNavClient(const rclcpp::NodeOptions & options)
: Node("simple_nav_client", options),
  goal_result_available_(false),
  goal_succeeded_(false)
{
  // 创建动作客户端
  this->action_client_ = rclcpp_action::create_client<navigatetopose>(
    this,
    "navigate_to_pose");

  // 等待动作服务器启动
  RCLCPP_INFO(this->get_logger(), "等待动作服务器启动...");
  if (! this->action_client_->wait_for_action_server(10s)) {
    RCLCPP_ERROR(this->get_logger(), "动作服务器启动超时！");
    return;
  }
  RCLCPP_INFO(this->get_logger(), "动作服务器已连接");
}

bool SimpleNavClient::sendGoal(double x, double y, double yaw)
{
  if (! this->action_client_) {
    RCLCPP_ERROR(this->get_logger(), "动作客户端未初始化");
    return false;
  }

  if (! this->action_client_->wait_for_action_server(1s)) {
    RCLCPP_ERROR(this->get_logger(), "动作服务器不可用");
    return false;
  }

  // 创建目标消息
  auto goal_msg = NavigateToPose::Goal();
  goal_msg.pose.header.stamp = this->now();
  goal_msg.pose.header.frame_id = "map";

  // 设置目标位置
  goal_msg.pose.pose.position.x = x;
  goal_msg.pose.pose.position.y = y;
  goal_msg.pose.pose.position.z = 0.0;

  // 从 yaw 角度计算四元数
  goal_msg.pose.pose.orientation.x = 0.0;
  goal_msg.pose.pose.orientation.y = 0.0;
  goal_msg.pose.pose.orientation.z = sin(yaw / 2.0);
  goal_msg.pose.pose.orientation.w = cos(yaw / 2.0);

  // 设置行为树
  goal_msg.behavior_tree = "";

  RCLCPP_INFO(this->get_logger(), "发送导航目标: (%.2f, %.2f, %.2f)", x, y, yaw);

  // 发送目标
  auto send_goal_options = rclcpp_action::Client<navigatetopose id="SRdrppWn0av8FIg7sM2li13igvb">::SendGoalOptions();
  send_goal_options.goal_response_callback =
    std::bind(&SimpleNavClient::goal_response_callback, this, std::placeholders::_1);
  send_goal_options.feedback_callback =
    std::bind(&SimpleNavClient::feedback_callback, this, std::placeholders::_1, std::placeholders::_2);
  send_goal_options.result_callback =
    std::bind(&SimpleNavClient::result_callback, this, std::placeholders::_1);

  auto goal_handle_future = this->action_client_->async_send_goal(goal_msg, send_goal_options);

  // 重置状态
  goal_result_available_ = false;
  goal_succeeded_ = false;

  return true;
}

bool SimpleNavClient::waitForResult(std::chrono::seconds timeout)
{
  auto start_time = std::chrono::steady_clock::now();

  while (! goal_result_available_) {
    rclcpp::spin_some(this->shared_from_this());

    auto current_time = std::chrono::steady_clock::now();
    if (current_time - start_time > timeout) {
      RCLCPP_WARN(this->get_logger(), "等待导航结果超时");
      return false;
    }

    std::this_thread::sleep_for(100ms);
  }

  return goal_succeeded_;
}

void SimpleNavClient::goal_response_callback(const GoalHandleNavigateToPose::SharedPtr & goal_handle)
{
  if (! goal_handle) {
    RCLCPP_ERROR(this->get_logger(), "目标被拒绝");
  } else {
    RCLCPP_INFO(this->get_logger(), "目标被接受，开始导航");
    this->goal_handle_ = goal_handle;
  }
}

void SimpleNavClient::feedback_callback(
  GoalHandleNavigateToPose::SharedPtr,
  const std::shared_ptr<const navigatetopose::feedback="" id="XmOKpFjh8ajxrRgj4VnlijCXgrg"> feedback)
{
  RCLCPP_INFO(
    this->get_logger(),
    "当前位置: (%.2f, %.2f)",
    feedback->current_pose.pose.position.x,
    feedback->current_pose.pose.position.y);
}

void SimpleNavClient::result_callback(const GoalHandleNavigateToPose::WrappedResult & result)
{
  goal_result_available_ = true;

  switch (result.code) {
    case rclcpp_action::ResultCode::SUCCEEDED:
      RCLCPP_INFO(this->get_logger(), "导航成功完成！");
      goal_succeeded_ = true;
      break;
    case rclcpp_action::ResultCode::ABORTED:
      RCLCPP_ERROR(this->get_logger(), "导航被中止");
      goal_succeeded_ = false;
      break;
    case rclcpp_action::ResultCode::CANCELED:
      RCLCPP_WARN(this->get_logger(), "导航被取消");
      goal_succeeded_ = false;
      break;
    default:
      RCLCPP_ERROR(this->get_logger(), "导航失败，未知错误");
      goal_succeeded_ = false;
      break;
  }
}